package com.gitee.wsl.mathematics.matrix.mat3.ext

import com.gitee.wsl.ext.number.clamp
import com.gitee.wsl.math.radiansToDegrees
import com.gitee.wsl.mathematics.geometry.EulerOrder
import com.gitee.wsl.mathematics.matrix.mat3.Mat3f
import com.gitee.wsl.mathematics.vector.vec3.MutableVec3f
import kotlin.math.asin

/**
 * Returns the euler angle representation of this matrix's rotation component. Angles are returned in degrees
 * in the given [eulersDeg] vector. Euler angle order can bes set via [order], default is ZYX.
 */
fun Mat3f.getEulerAngles(eulersDeg: MutableVec3f = MutableVec3f(), order: EulerOrder = EulerOrder.ZYX): MutableVec3f {
    var sx = sqrt(m00*m00 + m10*m10 + m20*m20)
    val sy = sqrt(m01*m01 + m11*m11 + m21*m21)
    val sz = sqrt(m02*m02 + m12*m12 + m22*m22)
    if (det < 0f) {
        sx *= -1f
    }

    val r00 = m00 / sx; val r01 = m01 / sy; val r02 = m02 / sz
    val r10 = m10 / sx; val r11 = m11 / sy; val r12 = m12 / sz
    val r20 = m20 / sx; val r21 = m21 / sy; val r22 = m22 / sz

    when (order) {
        EulerOrder.XYZ -> {
            eulersDeg.y = asin(r02.clamp(-1f, 1f)).radiansToDegrees()
            if (abs(r02) < 0.9999999f) {
                eulersDeg.x = atan2(-r12, r22).radiansToDegrees()
                eulersDeg.z = atan2(-r01, r00).radiansToDegrees()
            } else {
                eulersDeg.x = atan2(r21, r11).radiansToDegrees()
                eulersDeg.z = 0f
            }
        }
        EulerOrder.XZY -> {
            eulersDeg.z = asin(-r01.clamp(-1f, 1f)).radiansToDegrees()
            if (abs(r01) < 0.9999999f) {
                eulersDeg.x = atan2(r21, r11).radiansToDegrees()
                eulersDeg.y = atan2(r02, r00).radiansToDegrees()
            } else {
                eulersDeg.x = atan2(-r12, r22).radiansToDegrees()
                eulersDeg.y = 0f
            }
        }
        EulerOrder.YXZ -> {
            eulersDeg.x = asin(-r12.clamp(-1f, 1f)).radiansToDegrees()
            if (abs(r12) < 0.9999999f) {
                eulersDeg.y = atan2(r02, r22).radiansToDegrees()
                eulersDeg.z = atan2(r10, r11).radiansToDegrees()
            } else {
                eulersDeg.y = atan2(-r20, r00).radiansToDegrees()
                eulersDeg.z = 0f
            }
        }
        EulerOrder.YZX -> {
            eulersDeg.z = asin(r10.clamp(-1f, 1f)).radiansToDegrees()
            if (abs(r10) < 0.9999999f) {
                eulersDeg.x = atan2(-r12, r11).radiansToDegrees()
                eulersDeg.y = atan2(-r20, r00).radiansToDegrees()
            } else {
                eulersDeg.x = 0f
                eulersDeg.y = atan2(r02, r22).radiansToDegrees()
            }
        }
        EulerOrder.ZXY -> {
            eulersDeg.x = asin(r21.clamp(-1f, 1f)).radiansToDegrees()
            if (abs(r21) < 0.9999999f) {
                eulersDeg.y = atan2(-r20, r22).radiansToDegrees()
                eulersDeg.z = atan2(-r01, r11).radiansToDegrees()
            } else {
                eulersDeg.y = 0f
                eulersDeg.z = atan2(r10, r00).radiansToDegrees()
            }
        }
        EulerOrder.ZYX -> {
            eulersDeg.y = asin(-r20.clamp(-1f, 1f)).radiansToDegrees()
            if (abs(r20) < 0.9999999f) {
                eulersDeg.x = atan2(r21, r22).radiansToDegrees()
                eulersDeg.z = atan2(r10, r00).radiansToDegrees()
            } else {
                eulersDeg.x = 0f
                eulersDeg.z = atan2(-r01, r11).radiansToDegrees()
            }
        }
    }

    return eulersDeg
}